#include "Element_plane62_BT.h"

#include "Node_Base.h"

#include "Material_Base.h"

#include "Section_beam_BT.h"

void Element_plane62_BT::setElement()
{
	parameterlist.resize(m_pNode[0].lock()->m_parameter.size());
	for (int i = 0; i < parameterlist.size(); i++)
	{
		parameterlist[i].resize(m_pNode.size());
	}
	for (int i = 0; i < m_pNode.size(); i++)
	{
		for (int j = 0; j < parameterlist.size(); j++)
		{
			parameterlist[j][i] = m_pNode[i].lock()->m_parameter[j];
		}
	}
}

void Element_plane62_BT::coutElement()
{

}

void Element_plane62_BT::foutElement(std::ofstream& fout)
{
	fout << std::left << std::setw(20) << m_id;
	for (int i = 0; i < m_pNode.size(); i++)
	{
		fout << std::left << std::setw(20) << m_pNode[i].lock()->m_id;
	}
	fout << std::endl;
}

int Element_plane62_BT::getDOFNode() const
{
	for (int i = 0; i < 6; i++)
	{
		m_pNode[i].lock()->m_DOF.resize(5);
	}
	return 5;
}

bool Element_plane62_BT::get_Ke(Eigen::MatrixXd& ke)
{
	ke = Eigen::MatrixXd::Zero(6, 6);
	Eigen::VectorXd dNy, dNz;
	for (int i = 0; i < 4; i++)
	{
		double det = get_dNyz(ci(i), si(i), dNy, dNz);
		ke += wi(i) * det * (dNy * dNy.transpose() + dNz * dNz.transpose());
	}
	return true;
}

void Element_plane62_BT::get_Load(Eigen::VectorXd& Feq)
{
	Feq = Eigen::VectorXd::Zero(18);
	Eigen::VectorXd Feq1, Feq2, Feq3;
	Feq1 = Eigen::VectorXd::Zero(6);
	Feq2 = Eigen::VectorXd::Zero(6);
	Feq3 = Eigen::VectorXd::Zero(6);
	Eigen::VectorXd dNy, dNz, N;
	double m_v = m_pSection.lock()->m_pMaterial.lock()->m_v;
	std::vector<double> I_coord = m_pSection.lock()->I_coord;
	for (int i = 0; i < 4; i++)
	{
		double det = get_dNyz(ci(i), si(i), dNy, dNz);
		get_N(ci(i), si(i), N);
		double y = get_y(N);
		double z = get_z(N);
		double hy = -m_v * (I_coord[2] * (y * y - z * z) / 2 - I_coord[1] * y * z);
		double dy = m_v * (I_coord[0] * (y * y - z * z) / 2 - I_coord[2] * y * z);
		double hz = -m_v * (I_coord[2] * y * z + I_coord[1] * (y * y - z * z) / 2);
		double dz = m_v * (I_coord[0] * y * z + I_coord[2] * (y * y - z * z) / 2);
		Feq1 += (det * wi(i)) * (hy * dNy + hz * dNz - 2 * (1 + m_v) * (I_coord[2] * y - I_coord[1] * z) * N);
		Feq2 += (det * wi(i)) * (dy * dNy + dz * dNz - 2 * (1 + m_v) * (I_coord[2] * z - I_coord[0] * y) * N);
		Feq3 += (det * wi(i)) * (z * dNy - y * dNz);
		for (int j = 0; j < 6; j++)
		{
			Feq(3 * j) = Feq1(j);
			Feq(3 * j + 1) = Feq2(j);
			Feq(3 * j + 2) = Feq3(j);
		}
	}
}

void Element_plane62_BT::get_Load2(Eigen::VectorXd& Feq)
{
	Feq = Eigen::VectorXd::Zero(12);
	Eigen::VectorXd Feq1, Feq2;
	Feq1 = Eigen::VectorXd::Zero(6);
	Feq2 = Eigen::VectorXd::Zero(6);
	Eigen::VectorXd dNy, dNz, N, We;
	We = parameterlist[2];
	double yc = m_pSection.lock()->coord_c[0];
	double zc = m_pSection.lock()->coord_c[1];
	std::vector<double> I_coord = m_pSection.lock()->I_coord;
	for (int i = 0; i < 4; i++)
	{
		double det = get_dNyz(ci(i), si(i), dNy, dNz);
		get_N(ci(i), si(i), N);
		Feq1 += (det * wi(i)) * (N * (We.dot(dNy) - zc));
		Feq2 += (det * wi(i)) * (N * (We.dot(dNz) + yc));
		for (int j = 0; j < 6; j++)
		{
			Feq(2 * j) = Feq1(j);
			Feq(2 * j + 1) = Feq2(j);
		}
	}
}

void Element_plane62_BT::Assemble(std::vector<Eigen::VectorXd>& F1, std::vector<Eigen::VectorXd>& F2)
{
	Eigen::VectorXd Feq;
	get_Load(Feq);
	int m_nFixed = F1[1].size();
	for (int i = 0; i < (Feq.size() / 3); i++)
	{
		for (int j = 0; j < F1.size() - 2; j++)
		{
			auto DOF = m_pNode[i].lock()->m_DOF[j];
			if (DOF < m_nFixed)
			{
				F1[j][DOF] += Feq[(F1.size() - 2) * i + j];
			}
			else
			{
				F2[j][DOF - m_nFixed] += Feq[(F2.size() - 2) * i + j];
			}
		}
	}
}

void Element_plane62_BT::Assemble2(std::vector<Eigen::VectorXd>& F1, std::vector<Eigen::VectorXd>& F2)
{
	Eigen::VectorXd Feq;
	get_Load2(Feq);
	for (int i = 0; i < (Feq.size() / 2); i++)
	{
		for (int j = 3; j < F1.size(); j++)
		{
			auto DOF = m_pNode[i].lock()->m_DOF[j];
			if (DOF < F1[j].size())
			{
				F1[j][DOF] += Feq[2 * i + j - 3];
			}
			else
			{
				F2[j][DOF - F1[j].size()] += Feq[2 * i + j - 3];
			}
		}
	}
}

void Element_plane62_BT::get_A()
{
	hammer_integral2D();

	Eigen::VectorXd dNy, dNz, N;
	double& A = m_pSection.lock()->A;
	for (int i = 0; i < 4; i++)
	{
		double det = get_dNyz(ci(i), si(i), dNy, dNz);
		A += (det * wi(i));
	}
}

void Element_plane62_BT::get_S()
{
	std::vector<double>& S_coord = m_pSection.lock()->S_coord;
	Eigen::VectorXd dNy, dNz, N;
	for (int i = 0; i < 4; i++)
	{
		double det = get_dNyz(ci(i), si(i), dNy, dNz);
		get_N(ci(i), si(i), N);
		double y = get_y(N);
		double z = get_z(N);
		S_coord[0] += (det * wi(i)) * z;
		S_coord[1] += (det * wi(i)) * y;
	}
}

void Element_plane62_BT::get_I()
{
	std::vector<double>& I_coord = m_pSection.lock()->I_coord;
	Eigen::VectorXd dNy, dNz, N;
	for (int i = 0; i < 4; i++)
	{
		double det = get_dNyz(ci(i), si(i), dNy, dNz);
		get_N(ci(i), si(i), N);
		double y = get_y(N);
		double z = get_z(N);
		I_coord[0] += (det * wi(i)) * (z * z);
		I_coord[1] += (det * wi(i)) * (y * y);
		I_coord[2] += (det * wi(i)) * (y * z);
	}
}

void Element_plane62_BT::get_Nodeparameter(std::vector<Eigen::VectorXd>& x1, std::vector<Eigen::VectorXd>& x2)
{
	parameterlist.resize(m_pNode[0].lock()->m_DOF.size());
	for (int i = 0; i < parameterlist.size(); i++)
	{
		parameterlist[i].resize(m_pNode.size());
	}
	for (int i = 0; i < m_pNode.size(); i++)
	{
		m_pNode[i].lock()->m_parameter.resize(parameterlist.size());
		int DOFNode = m_pNode[i].lock()->m_DOF[0];
		for (int j = 0; j < parameterlist.size()-2; j++)
		{
			if (DOFNode < x1[j].size())
			{
				parameterlist[j][i] = x1[j][DOFNode];
				m_pNode[i].lock()->m_parameter[j] = x1[j][DOFNode];
			}
			else
			{
				parameterlist[j][i] = x2[j][DOFNode - x1[j].size()];
				m_pNode[i].lock()->m_parameter[j] = x2[j][DOFNode - x1[j].size()];
			}
		}
	}
}

void Element_plane62_BT::get_Nodeparameter2(std::vector<Eigen::VectorXd>& x1, std::vector<Eigen::VectorXd>& x2)
{
	for (int i = 0; i < m_pNode.size(); i++)
	{
		m_pNode[i].lock()->m_parameter.resize(parameterlist.size());
		for (int j = 3; j < parameterlist.size(); j++)
		{
			int DOFNode = m_pNode[i].lock()->m_DOF[j];
			if (DOFNode < x1[j].size())
			{
				parameterlist[j][i] = x1[j][DOFNode];
				m_pNode[i].lock()->m_parameter[j] = x1[j][DOFNode];
			}
			else
			{
				parameterlist[j][i] = x2[j][DOFNode - x1[j].size()];
				m_pNode[i].lock()->m_parameter[j] = x2[j][DOFNode - x1[j].size()];
			}
		}
	}
}

void Element_plane62_BT::get_Sw()
{
	Eigen::VectorXd dNy, dNz, N, We;
	We = parameterlist[2];
	std::vector<double>& Iw_coord = m_pSection.lock()->Iw_coord;
	for (int i = 0; i < 4; i++)
	{
		double det = get_dNyz(ci(i), si(i), dNy, dNz);
		get_N(ci(i), si(i), N);
		double y = get_y(N);
		double z = get_z(N);
		double W = We.dot(N);
		Iw_coord[2] += (det * wi(i)) * (W);
	}
}

void Element_plane62_BT::get_Sw(double& Sw)
{
	Eigen::VectorXd dNy, dNz, N, We;
	hammer_integral2D();
	We = parameterlist[2];
	std::vector<double> coord_s = m_pSection.lock()->coord_s1;
	for (int i = 0; i < 4; i++)
	{
		double det = get_dNyz(ci(i), si(i), dNy, dNz);
		get_N(ci(i), si(i), N);
		double y = get_y(N);
		double z = get_z(N);
		double W = We.dot(N);
		Sw += (det * wi(i)) * (W - coord_s[1] * y + coord_s[0] * z);
	}
}

void Element_plane62_BT::get_J()
{
	Eigen::VectorXd dNy, dNz, N, We;
	We = parameterlist[2];
	double& J = m_pSection.lock()->J;
	std::vector<double>& Iw_coord = m_pSection.lock()->Iw_coord;
	for (int i = 0; i < 4; i++)
	{
		double det = get_dNyz(ci(i), si(i), dNy, dNz);
		get_N(ci(i), si(i), N);
		double y = get_y(N);
		double z = get_z(N);
		double W = We.dot(N);
		double w_y = We.dot(dNy);
		double w_z = We.dot(dNz);
		J += (det * wi(i)) * ((w_z + y) * y - (w_y - z) * z);
		Iw_coord[0] += (det * wi(i)) * (y * W);
		Iw_coord[1] += (det * wi(i)) * (z * W);
	}
}

void Element_plane62_BT::get_Iw()
{
	Eigen::VectorXd dNy, dNz, N, We;
	We = parameterlist[2];
	double& J = m_pSection.lock()->J;
	std::vector<double> coord_s = m_pSection.lock()->coord_s1;
	std::vector<double>& Iw_coord = m_pSection.lock()->Iw_coord;
	for (int i = 0; i < 4; i++)
	{
		double det = get_dNyz(ci(i), si(i), dNy, dNz);
		get_N(ci(i), si(i), N);
		double y = get_y(N);
		double z = get_z(N);
		double W = We.dot(N);
		Iw_coord[3] += (det * wi(i)) * ((W - coord_s[1] * y + coord_s[0] * z) * (W - coord_s[1] * y + coord_s[0] * z));
	}
}

void Element_plane62_BT::get_a()
{
	double m_v = m_pSection.lock()->m_pMaterial.lock()->m_v;
	double A = m_pSection.lock()->A;
	std::vector<double> I_coord = m_pSection.lock()->I_coord;
	Eigen::Matrix2d& a1 = m_pSection.lock()->a1;

	double delta = 2 * (1 + m_v) * (I_coord[0] * I_coord[1] - I_coord[2] * I_coord[2]);

	Eigen::VectorXd dNy, dNz, N, phi, psi;
	phi = parameterlist[0];
	psi = parameterlist[1];
	for (int i = 0; i < 4; i++)
	{
		double det = get_dNyz(ci(i), si(i), dNy, dNz);
		get_N(ci(i), si(i), N);
		double y = get_y(N);
		double z = get_z(N);
		double hy = -m_v * (I_coord[2] * (y * y - z * z) / 2 - I_coord[1] * y * z);
		double dy = m_v * (I_coord[0] * (y * y - z * z) / 2 - I_coord[2] * y * z);
		double hz = -m_v * (I_coord[2] * y * z + I_coord[1] * (y * y - z * z) / 2);
		double dz = m_v * (I_coord[0] * y * z + I_coord[2] * (y * y - z * z) / 2);
		a1(0, 0) += (det * wi(i)) * (A / (delta * delta)) * ((psi.dot(dNy) - dy) * (psi.dot(dNy) - dy) + (psi.dot(dNz) - dz) * (psi.dot(dNz) - dz));
		a1(1, 1) += (det * wi(i)) * (A / (delta * delta)) * ((phi.dot(dNy) - hy) * (phi.dot(dNy) - hy) + (phi.dot(dNz) - hz) * (phi.dot(dNz) - hz));
		a1(1, 0) += (det * wi(i)) * (A / (delta * delta)) * ((psi.dot(dNy) - dy) * (phi.dot(dNy) - hy) + (psi.dot(dNz) - dz) * (phi.dot(dNz) - hz));
	}
}

void Element_plane62_BT::get_coord_s()
{
	double m_v = m_pSection.lock()->m_pMaterial.lock()->m_v;
	double A = m_pSection.lock()->A;
	std::vector<double> I_coord = m_pSection.lock()->I_coord;
	std::vector<double> coord_s2 = m_pSection.lock()->coord_s2;

	double delta = 2 * (1 + m_v) * (I_coord[0] * I_coord[1] - I_coord[2] * I_coord[2]);

	Eigen::VectorXd dNy, dNz, N, phi, psi;
	phi = parameterlist[0];
	psi = parameterlist[1];
	for (int i = 0; i < 4; i++)
	{
		double det = get_dNyz(ci(i), si(i), dNy, dNz);
		get_N(ci(i), si(i), N);
		double y = get_y(N);
		double z = get_z(N);
		double hy = -m_v * (I_coord[2] * (y * y - z * z) / 2 - I_coord[1] * y * z);
		double dy = m_v * (I_coord[0] * (y * y - z * z) / 2 - I_coord[2] * y * z);
		double hz = -m_v * (I_coord[2] * y * z + I_coord[1] * (y * y - z * z) / 2);
		double dz = m_v * (I_coord[0] * y * z + I_coord[2] * (y * y - z * z) / 2);
		coord_s2[0] += (det * wi(i)) / delta * (y * (phi.dot(dNz) - hz) - z * (phi.dot(dNy) - hy));
		coord_s2[1] -= (det * wi(i)) / delta * (y * (psi.dot(dNz) - dz) - z * (psi.dot(dNy) - dy));
	}
}

void Element_plane62_BT::get_stress(Eigen::MatrixXd& in, std::vector<double>& Sforce, std::vector<double>& Scoord, std::vector<double>& Tangle)
{
	hammer_integral2D();

	double m_v = m_pSection.lock()->m_pMaterial.lock()->m_v;
	double E = m_pSection.lock()->m_pMaterial.lock()->E;
	double G = E / (2 * (1 + m_v));
	double Vy = Sforce[0];
	double Vz = Sforce[1];
	double y = Scoord[0] - m_pSection.lock()->coord_c[0];
	double z = Scoord[1] - m_pSection.lock()->coord_c[1];
	double sita = Tangle[0];
	double sita_x = Tangle[1];
	double weight = 0;

	std::vector<double> I_coord = m_pSection.lock()->I_coord;
	double delta = 2 * (1 + m_v) * (I_coord[0] * I_coord[1] - I_coord[2] * I_coord[2]);

	Eigen::VectorXd dNy, dNz, N, We, phi, psi, wxy, wxz, awxy, awxz, axy, axz;
	//awxy.resize(3);
	//awxz.resize(3);
	//axy.resize(3);
	//axz.resize(3);
	phi = parameterlist[0];
	psi = parameterlist[1];
	We = parameterlist[2];
	wxy = parameterlist[3];
	wxz = parameterlist[4];
	for (int i = 0; i < ci.size()-1; i++)
	{
		double det = get_dNyz(ci(i), si(i), dNy, dNz);
		get_N(ci(i), si(i), N);
		double y = get_y(N);
		double z = get_z(N);
		double hy = -m_v * (I_coord[2] * (y * y - z * z) / 2 - I_coord[1] * y * z);
		double dy = m_v * (I_coord[0] * (y * y - z * z) / 2 - I_coord[2] * y * z);
		double hz = -m_v * (I_coord[2] * y * z + I_coord[1] * (y * y - z * z) / 2);
		double dz = m_v * (I_coord[0] * y * z + I_coord[2] * (y * y - z * z) / 2);
		//awxy(i) = G * sita * (-z + We.dot(dNy));
		//awxz(i) = G * sita * (y + We.dot(dNz));
		//axy(i) = (Vz / delta) * (phi.dot(dNy) - hy) + (Vy / delta) * (psi.dot(dNy) - dy);
		//axz(i) = (Vz / delta) * (phi.dot(dNz) - hz) + (Vy / delta) * (psi.dot(dNz) - dz);
		weight += (det * wi(i));
	}
	//std::cout << awxy << std::endl;
	//std::cout << awxz << std::endl;
	//Eigen::MatrixXd N2;
	//N2.resize(6, 3);
	//for (int i = 0; i < 6; i++)
	//{
	//	exhammer();
	//	get_Nex(ci(i), si(i), zi(i), N);
	//	N2.row(i) = N;
	//}
	Eigen::VectorXd Node_id, vA, Node_y, Node_z, bwxy, bwxz, bxy, bxz;
	Node_id.resize(6);
	vA.resize(6);
	Node_y.resize(6);
	Node_z.resize(6);
	bwxy.resize(6);
	bwxz.resize(6);
	bxy.resize(6);
	bxz.resize(6);

	//bwxy = N2 * awxy;
	//bwxz = N2 * awxz;
	//bxy = N2 * axy;
	//bxz = N2 * axz;

	Nodeff();
	for (int i = 0; i < ci.size(); i++)
	{
		double det = get_dNyz(ci(i), si(i), dNy, dNz);
		get_N(ci(i), si(i), N);
		double y = get_y(N);
		double z = get_z(N);
		double hy = -m_v * (I_coord[2] * (y * y - z * z) / 2 - I_coord[1] * y * z);
		double dy = m_v * (I_coord[0] * (y * y - z * z) / 2 - I_coord[2] * y * z);
		double hz = -m_v * (I_coord[2] * y * z + I_coord[1] * (y * y - z * z) / 2);
		double dz = m_v * (I_coord[0] * y * z + I_coord[2] * (y * y - z * z) / 2);
		bwxy(i) = G * sita * (-z + We.dot(dNy));
		bwxz(i) = G * sita * (y + We.dot(dNz));
		bxy(i) = (Vz / delta) * (phi.dot(dNy) - hy) + (Vy / delta) * (psi.dot(dNy) - dy);
		bxz(i) = (Vz / delta) * (phi.dot(dNz) - hz) + (Vy / delta) * (psi.dot(dNz) - dz);
	}

	for (int i = 0; i < 6; i++)
	{
		Node_id(i) = m_pNode[i].lock()->m_id;
		Node_y(i) = m_pNode[i].lock()->m_Coord[0] + m_pSection.lock()->coord_c[0];
		Node_z(i) = m_pNode[i].lock()->m_Coord[1] + m_pSection.lock()->coord_c[1];
		vA(i) = weight;
		bwxy(i) -= E / (1 + m_v) * sita_x * wxy[i];
		bwxz(i) -= E / (1 + m_v) * sita_x * wxz[i];
	}

	in.resize(6, 8);
	in.col(0) = Node_id;
	in.col(1) = vA;
	in.col(2) = Node_y;
	in.col(3) = Node_z;
	in.col(4) = bwxy;
	in.col(5) = bwxz;
	in.col(6) = bxy;
	in.col(7) = bxz;
}

double Element_plane62_BT::get_w(double ci, double si)
{
	Eigen::VectorXd N;
	get_N(ci, si, N);
	return parameterlist[2].dot(N);
}
